import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.HashMap;

import path.Path;
import robot.Robot;
import config.Config;
import log.Log;


public class AIProjectMain {

	private static final String LOG_TAG = AIProjectMain.class.getSimpleName();

	public static void main(String[] args){
		HashMap<String, String> argsMap = new HashMap<String, String>();
		if(args.length == 0){
			System.out.println("Please enter path to the file containing the path :");
			InputStreamReader inp = new InputStreamReader(System.in); 
			BufferedReader br = new BufferedReader(inp);
			try {
				argsMap.put(Config.ARGS_PATH, br.readLine());
			} catch (IOException e) {
				e.printStackTrace();
			}
		}else{
			for(int i = 0; i < args.length - 1; i++){
				if(args[i].contains("--")){
					String key = args[i].substring(args[i].lastIndexOf("-") + 1);
					String value = args[i+1];
					argsMap.put(key, value);
				}
			}
		}
		if(argsMap.containsKey(Config.ARGS_DEBUG)){
			Config.DEBUG = Boolean.valueOf(argsMap.get(Config.ARGS_DEBUG));
		}
		if(argsMap.containsKey(Config.ARGS_MAX_POINT_TO_TAKE)){
			Config.MAX_POINT_TO_TAKE = Integer.valueOf(argsMap.get(Config.ARGS_MAX_POINT_TO_TAKE));
		}
		if(argsMap.containsKey(Config.ARGS_DYNAMIC_SPEED)){
			Config.DYNAMIC_SPEED = Boolean.valueOf(argsMap.get(Config.ARGS_DYNAMIC_SPEED));
		}
		if(argsMap.containsKey(Config.ARGS_AVOID_OB)){
			Config.AVOID_OB = Boolean.valueOf(argsMap.get(Config.ARGS_AVOID_OB));
		}
		if(argsMap.containsKey(Config.ARGS_SPEED)){
			Config.SET_SPEED = Double.valueOf(argsMap.get(Config.ARGS_SPEED));
		}
		
		
		Robot r = new Robot(argsMap);
		Path p = new Path(argsMap.get(Config.ARGS_PATH));
		r.setPathToFollow(p);
		int positionGoal = 0; 
		long start = System.currentTimeMillis();
		while(p.getPath().size() > 0){
			Log.i(LOG_TAG, "List size : " + p.getPath().size());
			r.getCurrentPose();
			positionGoal = p.getNextGoalPoint(r.getLookAheadDistance(), r);
			r.purePursuit(positionGoal);
		}
		long end = System.currentTimeMillis();
		System.out.println("TOTAL TIME = " + (end - start)/1000.0);
	}
}

